package edu.csusm.spotrobot;

import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.io.IIOPin;
import com.sun.spot.sensorboard.peripheral.Servo;

/**
 * Controls a servo
 *
 * @author Zebulon Evans
 */
public class SonarServo extends Thread {
    
    private EDemoBoard demo = EDemoBoard.getInstance();
    private Servo sonarServo; 
    /** The pin the servo is connected to on the SunSPOT */
    private int servoPin;
    /** The orientation that the servo is set to from 0-1 */
    private float orientation = 0.40f;
    /** How far the servo turns on each iteration */
    private float servoTurn = +0.02f;
    private int counter = 0;
    
    /**
     * Creates a new SonarServo
     *
     * @param int the pin that the servo is connected to on the SunSPOT
     */
    public SonarServo( int servoPinIn ){
        servoPin = servoPinIn;
        sonarServo = new Servo(demo.getIOPins()[servoPin]);
        sonarServo.setBounds(500, 2100);
        setOrientation(0.5f);
    }
    
    /**
     * The run function for the thread.  Keep sweeping back and forth.
     */
    public void run() {
        while(true){
                setOrientation( orientation );
                counter = 0;
                orientation += servoTurn;
            if( orientation >= 0.70f )
                servoTurn = -0.02f;
            if( orientation <= 0.30f )
                servoTurn = 0.02f;
            //}
            counter++;


            try {
                Thread.sleep(150);
            } catch (InterruptedException e) {
                //We've been interrupted: exit the thread
                return;
            }
        }
    }
    
    /**
     * Set the orientation of the servo.
     *
     * @param float the orientation of the servo from 0-1
     */
    public void setOrientation( float orientation ) {
        sonarServo.setPosition( orientation );
    }

}
